#include <gtest/gtest.h>
#include <hest.h>

TEST(HestTest, SimpleRansac) {
  std::vector<Eigen::Vector2d> points1 = {
      {0, 0},
      {0, 10},
      {10, 10},
      {10, 0},
  };
  std::vector<Eigen::Vector2d> points2 = {
      {10, 0},
      {10, 10},
      {20, 10},
      {20, 0},
  };

  Eigen::Matrix3d expected;
  expected << 1., 0., -10.,
      0., 1., 0.,
      0., 0., 1.;

  Eigen::Matrix3d H = hest::estimateHomographyPoints(points1, points2, false);
  H = H / H(2, 2);

  ASSERT_TRUE(expected.isApprox(H));
}


TEST(HestTest, LineDegeneratedExample2){
  std::vector<Eigen::Vector2d> points1 = {{35.4479,24.487549},{34.607758,95.47761},{52.905296,110.03256},{39.56109,24.37202},{38.883255,99.43943},{41.744396,16.97757},{10.,30.},{150.,38.},{73.,49.},{51.,52.},{103.,57.},{22.,66.},{107.,81.},{119.,83.},{126.,85.},{105.,104.},{119.,106.},{126.,108.},{21.,123.},{77.,124.},{103.,129.},{21.,136.},{50.,144.},{73.,144.},{60.,147.},{50.,159.},{49.,164.},{61.,168.},{66.,169.},{76.,170.},{103.,172.},{24.,182.},{39.,194.},{50.,194.},{39.,202.},{51.,205.},{24.,211.},{62.,214.},{78.,215.},{78.,235.},{53.,236.},{39.,260.},{38.,283.},{27.,284.},{51.,284.}};
  std::vector<Eigen::Vector2d> points2 = {{519.0558,221.83603},{531.64233,275.93222},{546.5064,283.8466},{521.49994,218.57434},{535.4986,276.9801},{521.49713,212.79076},{503.,232.},{621.,181.},{552.,226.},{536.,236.},{580.,222.},{517.,256.},{590.,238.},{602.,237.},{612.,235.},{595.,262.},{609.,259.},{618.,257.},{527.,302.},{574.,290.},{601.,287.},{529.,313.},{555.,315.},{576.,310.},{566.,314.},{559.,328.},{559.,335.},{569.,336.},{574.,336.},{587.,336.},{616.,333.},{542.,355.},{559.,368.},{569.,368.},{559.,373.},{571.,377.},{549.,385.},{585.,385.},{603.,386.},{609.,410.},{582.,411.},{576.,439.},{582.,466.},{571.,467.},{597.,469.}};
  std::vector<hest::LineSegment> line_segments1 = {{{35.4479,24.487549},{34.607758,95.47761}},{{0.228339,95.4836},{52.905296,110.03256}},{{-0.754628,5.128477},{39.56109,24.37202}},{{38.883255,99.43943},{-0.800544,85.09893}},{{41.744396,16.97757},{7.324293,0.875597}}};
  std::vector<hest::LineSegment> line_segments2 = {{{519.0558,221.83603},{531.64233,275.93222}},{{452.95325,283.62714},{546.5064,283.8466}},{{446.5011,218.49257},{521.49994,218.57434}},{{535.4986,276.9801},{452.7856,275.88098}},{{521.49713,212.79076},{425.48672,211.8378}}};
  Eigen::Matrix3d expected;
  expected << -3.145121e-01, -3.507000e-01, 6.490993e+02,
      -9.121188e-01, 6.454459e-01, 2.834298e+02,
      -2.195508e-03, -1.095938e-03, 1.318120e+00;

  expected = expected / expected(2, 2);

  std::vector<int> inlier_pts_ind, inlier_lin_ind;
  // This works
  // Eigen::Matrix3d H = hest::ransacPointHomography(points2, points1, 3, &inlier_pts_ind);

  // This doesn't work
  Eigen::Matrix3d H = hest::ransacPointLineHomography(points2, points1, line_segments2, line_segments1,
                                                      3, false, &inlier_pts_ind, &inlier_lin_ind);
  H = H / H(2, 2);
//  std::cout << "GT H: \n" << expected << std::endl;
//  std::cout << "Estimated H: \n" << H << std::endl;
//  std::cerr << "P Inliers: " << inlier_pts_ind.size() << std::endl;
//  std::cerr << "L Inliers: " << inlier_lin_ind.size() << std::endl;
  ASSERT_GE(inlier_pts_ind.size(), 4);
  ASSERT_GE(inlier_lin_ind.size(), 4);
  ASSERT_TRUE(expected.isApprox(H, 0.3));
}

TEST(HestTest, LineDegeneratedExample9) {
  std::vector<hest::LineSegment> line_segments1 = {
      {{5.192106e+02, 4.867362e-01}, {5.204780e+02, 2.595050e+02}},
      {{4.479425e+02, 4.150234e+01}, {4.482446e+02, 1.135011e+02}},
      {{4.616704e+02, 3.449778e+01}, {4.619050e+02, 5.250775e+01}},
      {{5.581827e+02, 2.815216e+02}, {5.577719e+02, 2.945086e+02}},
      {{5.626058e+02, 2.990449e+02}, {5.731703e+02, 2.929369e+02}},
      {{5.836385e+02, 2.825127e+02}, {5.735563e+02, 2.810983e+02}},
      {{5.890233e+02, 2.843566e+02}, {5.888921e+02, 2.945414e+02}},
      {{5.850159e+02, 2.762466e+02}, {5.890233e+02, 2.843566e+02}},
      {{5.687343e+02, 2.700905e+02}, {5.767085e+02, 2.719478e+02}},
      {{5.153138e+02, 2.632680e+02}, {5.164785e+02, 2.723748e+02}},
      {{4.551842e+02, 3.073088e+01}, {4.551842e+02, 3.073088e+01}}};
  std::vector<hest::LineSegment> line_segments2 = {
      {{1.458551e+02, 7.764498e-02}, {3.616091e+01, 4.292095e+02}},
      {{1.134076e+01, 1.928018e+02}, {2.046029e+00, 2.186960e+02}},
      {{3.848630e+01, 1.694939e+02}, {3.523196e+01, 1.768249e+02}},
      {{7.399129e+01, 4.536284e+02}, {6.951060e+01, 4.707643e+02}},
      {{7.550682e+01, 4.745088e+02}, {8.624015e+01, 4.661657e+02}},
      {{1.003509e+02, 4.509732e+02}, {9.034862e+01, 4.519501e+02}},
      {{1.055421e+02, 4.532002e+02}, {1.019553e+02, 4.665906e+02}},
      {{1.005632e+02, 4.428673e+02}, {1.055421e+02, 4.532002e+02}},
      {{8.748553e+01, 4.399435e+02}, {9.555807e+01, 4.397336e+02}},
      {{2.901524e+01, 4.340829e+02}, {2.490227e+01, 4.450811e+02}},
      {{-1.639204e-01, 1.513650e+02}, {3.313360e+01, 1.590811e+02}}};
  Eigen::Matrix3d expected;
  expected << 1.155015e+00, -3.301729e-01, -4.851988e+02,
      8.543131e-01, 6.717014e-01, -2.577521e+02,
      2.118689e-03, -1.031790e-03, 8.202098e-03;

  expected = expected / expected(2, 2);

  std::vector<int> inlier_pts_ind, inlier_lin_ind;
  // This works
  Eigen::Matrix3d H = hest::ransacLineHomography(line_segments2, line_segments1, 3, false, &inlier_pts_ind);

  H = H / H(2, 2);
  std::cout << "GT H: \n" << expected << std::endl;
  std::cout << "Estimated H: \n" << H << std::endl;
  std::cerr << "P Inliers: " << inlier_pts_ind.size() << std::endl;
  std::cerr << "L Inliers: " << inlier_lin_ind.size() << std::endl;
//  ASSERT_GE(inlier_pts_ind.size(), 4);
//  ASSERT_GE(inlier_lin_ind.size(), 4);
//  ASSERT_TRUE(expected.isApprox(H, 0.3));
}

TEST(HestTest, ProblematicPair) {
  //TODO
  std::vector<Eigen::Vector2d> points1 = {
      {520.4780, 259.5050}, {519.8615, 274.5208}, {455.1842, 30.7309}, {441.3742, 45.1260}, {450.3365, 33.5390},
      {463.7924, 90.4924}, {464.3365, 111.5042}, {461.6704, 34.4978}, {461.9050, 52.5077}, {564.4286, 275.5515},
      {558.1827, 281.5216}, {557.7719, 294.5086}, {562.6058, 299.0449}, {573.1703, 292.9369}, {576.7085, 271.9478},
      {585.0159, 276.2466}, {583.6385, 282.5127}, {573.5563, 281.0983}, {589.0233, 284.3566}, {588.8921, 294.5414},
      {561.3786, 264.7051}, {568.7343, 270.0905}, {515.3138, 263.2680}, {516.4785, 272.3748}, {559.0000, 23.0000},
      {488.0000, 24.0000}, {587.0000, 46.0000}, {562.0000, 50.0000}, {585.0000, 70.0000}, {568.0000, 73.0000},
      {487.0000, 88.0000}, {480.0000, 89.0000}, {616.0000, 90.0000}, {598.0000, 120.0000}, {590.0000, 121.0000},
      {617.0000, 141.0000}, {532.0000, 151.0000}, {521.0000, 152.0000}, {616.0000, 166.0000}, {532.0000, 172.0000},
      {591.0000, 192.0000}, {501.0000, 195.0000}, {496.0000, 198.0000}, {589.0000, 214.0000}, {617.0000, 214.0000},
      {499.0000, 236.0000}, {590.0000, 247.0000}, {575.0000, 261.0000}, {582.0000, 261.0000}, {520.0000, 266.0000},
      {545.0000, 280.0000}, {522.0000, 296.0000}};
  std::vector<Eigen::Vector2d> points2 = {
      {36.1609, 429.2095}, {30.6303, 449.2665}, {33.1336, 159.0811}, {11.5802, 165.1142}, {26.2506, 162.4109},
      {24.0000, 224.0000}, {11.1666, 246.0668}, {38.4863, 169.4939}, {35.2320, 176.8249}, {79.0000, 449.0000},
      {73.9913, 453.6284}, {69.5106, 470.7643}, {75.5068, 474.5088}, {86.2402, 466.1657}, {95.5581, 439.7336},
      {100.5632, 442.8673}, {100.3509, 450.9732}, {90.3486, 451.9501}, {105.5421, 453.2002}, {101.9553, 466.5906},
      {82.0000, 433.0000}, {87.4855, 439.9435}, {29.0152, 434.0829}, {24.9023, 445.0811}, {132.0000, 201.0000},
      {70.0000, 172.0000}, {147.0000, 226.0000}, {130.0000, 223.0000}, {145.0000, 246.0000}, {131.0000, 245.0000},
      {55.0000, 230.0000}, {38.0000, 227.0000}, {161.0000, 270.0000}, {146.0000, 290.0000}, {140.0000, 290.0000},
      {156.0000, 310.0000}, {81.0000, 305.0000}, {70.0000, 303.0000}, {151.0000, 333.0000}, {76.0000, 327.0000},
      {126.0000, 354.0000}, {34.0000, 347.0000}, {26.0000, 349.0000}, {119.0000, 376.0000}, {143.0000, 378.0000},
      {12.0000, 398.0000}, {114.0000, 411.0000}, {99.0000, 427.0000}, {104.0000, 427.0000}, {31.0000, 440.0000},
      {60.0000, 454.0000}, {26.9995, 478.8882}};
  std::vector<hest::LineSegment> line_segments1 = {
      {{5.2667e+02, 4.7851e+02}, {5.2260e+02, 4.9917e-01}}, {{5.1921e+02, 4.8674e-01}, {5.2048e+02, 2.5951e+02}},
      {{4.5536e+02, 1.8350e+02}, {4.5034e+02, 3.3539e+01}}, {{4.4972e+02, 1.7749e+02}, {4.4137e+02, 4.5126e+01}},
      {{4.6167e+02, 3.4498e+01}, {4.6190e+02, 5.2508e+01}}, {{5.5818e+02, 2.8152e+02}, {5.5777e+02, 2.9451e+02}},
      {{5.6261e+02, 2.9904e+02}, {5.7317e+02, 2.9294e+02}}, {{5.8364e+02, 2.8251e+02}, {5.7356e+02, 2.8110e+02}},
      {{5.8902e+02, 2.8436e+02}, {5.8889e+02, 2.9454e+02}}, {{5.8502e+02, 2.7625e+02}, {5.8902e+02, 2.8436e+02}},
      {{5.6873e+02, 2.7009e+02}, {5.7671e+02, 2.7195e+02}}, {{5.1531e+02, 2.6327e+02}, {5.1648e+02, 2.7237e+02}},
      {{4.5518e+02, 3.0731e+01}, {4.5518e+02, 3.0731e+01}}};

  std::vector<hest::LineSegment> line_segments2 = {
      {{2.7000e+01, 4.7889e+02}, {1.5088e+02, 3.4039e-01}}, {{1.4586e+02, 7.7645e-02}, {3.6161e+01, 4.2921e+02}},
      {{6.1368e+00, 2.1873e+02}, {2.6251e+01, 1.6241e+02}}, {{-2.1200e+00, 2.1276e+02}, {1.0443e+01, 1.6820e+02}},
      {{3.8486e+01, 1.6949e+02}, {3.5232e+01, 1.7682e+02}}, {{7.3991e+01, 4.5363e+02}, {6.9511e+01, 4.7076e+02}},
      {{7.5507e+01, 4.7451e+02}, {8.6240e+01, 4.6617e+02}}, {{1.0035e+02, 4.5097e+02}, {9.0349e+01, 4.5195e+02}},
      {{1.0554e+02, 4.5320e+02}, {1.0196e+02, 4.6659e+02}}, {{1.0056e+02, 4.4287e+02}, {1.0554e+02, 4.5320e+02}},
      {{8.7486e+01, 4.3994e+02}, {9.5558e+01, 4.3973e+02}}, {{2.9015e+01, 4.3408e+02}, {2.4902e+01, 4.4508e+02}},
      {{3.2500e+01, 1.6416e+02}, {2.8202e+01, 1.7674e+02}}};

  std::vector<int> inlier_pts_ind, inlier_lin_ind;
  Eigen::Matrix3d H = hest::ransacPointLineHomography(points1, points2, line_segments1, line_segments2,
                                                      3, false, &inlier_pts_ind, &inlier_lin_ind);
//  Eigen::Matrix3d H = hest::ransacPointLineHomography(points2, points1, line_segments2, line_segments1,
//                                                      3, &inlier_pts_ind, &inlier_lin_ind);
  std::cout << "Estimated H: \n" << H << std::endl;
  std::cerr << "P Inliers: " << inlier_pts_ind.size() << std::endl;
  std::cerr << "L Inliers: " << inlier_lin_ind.size() << std::endl;
}

int main(int argc, char **argv) {
  // Se the maximum log level
  // Init and run the regression tests
  ::testing::InitGoogleTest(&argc, argv);
  int ret = RUN_ALL_TESTS();
  return ret;
}